# coding:utf-8

import cv2 as cv
import numpy as np
import time
import serial
# import some something.

angleList = []
angleLast = 0

robotStart = True
findedTarg = False
sleepTime = 100


# def serialSendData(data):
#     if ser.isOpen == False:  # 串口启动判断
#         ser.open()  # 串口启动
#     s = str('.')
#     ser.write(data + s.encode())  # 串口发送数据
#     # time.sleep(0.2)
#     # response = ser.readline()


def imRead(cap):
    ret, img = cap.read()  # 读取img图像
    if not ret:
        print("Can't receive frame (stream end?). Exiting ...")
        return None
    else:
        return img


def HoughLines(imgThreshed):
    edges = cv.Canny(imgThreshed, 50, 150, apertureSize=3)
    cv.imshow('edges', edges)
    lines = cv.HoughLinesP(edges, 1, np.pi / 180, 22, minLineLength=15, maxLineGap=10)
    return lines


def HoughCircles(imgThreshed):
    edges = cv.Canny(imgThreshed, 50, 150, apertureSize=3)
    circles = cv.HoughCircles(edges, cv.HOUGH_GRADIENT, 1, 30, param1=100, param2=25, minRadius=0, maxRadius=40)
    return circles


def imFiltering(imgGray):
    # 图像滤波
    # imgFilted = cv.filter2D(imgGrey,-1,kernel)
    imgFiltered = cv.GaussianBlur(imgGray, (5, 5), 0)  # 高斯滤波
    return imgFiltered


def robotDirect(imgBinary, robotStart):
    area = []
    angle = 0
    global angleLast

    contours, hierarchy = cv.findContours(imgBinary, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE)
    if len(contours) > 0:
        for index in range(len(contours)):
            area.append(cv.contourArea(contours[index]))
        indexMax = area.index(max(area))
        cnt = contours[indexMax]
        rect = cv.minAreaRect(cnt)
        box = cv.boxPoints(rect)
        box = np.int0(box)
        # print("box",box)
        cv.drawContours(img, [box], 0, (0, 0, 255), 2)  # 绘制识别区矩形线框

        # left_point_x = np.min(box[:, 0])
        # right_point_x = np.max(box[:, 0])
        # top_point_y = np.min(box[:, 1])
        # bottom_point_y = np.max(box[:, 1])
        # left_point_y = box[:, 1][np.where(box[:, 0] == left_point_x)][0]
        # right_point_y = box[:, 1][np.where(box[:, 0] == right_point_x)][0]
        # top_point_x = box[:, 0][np.where(box[:, 1] == top_point_y)][0]
        # bottom_point_x = box[:, 0][np.where(box[:, 1] == bottom_point_y)][0]

        # leftP = (left_point_x,left_point_y)
        # rightP = (right_point_x,right_point_y)
        # topP =(top_point_x,top_point_y)
        # BottomP = (bottom_point_x,bottom_point_y)

        # cv.line(img,topP,rightP,(150,0,150),2)

        length = np.square(box[1][0] - box[0][0]) + np.square(box[1][1] - box[0][1])
        width = np.square(box[3][0] - box[0][0]) + np.square(box[3][1] - box[0][1])
        if length >= width:
            xs = int((box[0][0] + box[3][0]) / 2)
            ys = int((box[0][1] + box[3][1]) / 2)
            xe = int((box[1][0] + box[2][0]) / 2)
            ye = int((box[1][1] + box[2][1]) / 2)
            # cv.line(img, (xs,ys),(xe,ye),(0,255,0),2)
        else:
            xs = int((box[0][0] + box[1][0]) / 2)
            ys = int((box[0][1] + box[1][1]) / 2)
            xe = int((box[2][0] + box[3][0]) / 2)
            ye = int((box[2][1] + box[3][1]) / 2)
            # cv.line(img, (xs,ys),(xe,ye),(0,255,0),2)
        if ys > ye:
            DirectionVec = np.array([[xs, ys], [xe, ye]], np.int32)
        else:
            DirectionVec = np.array([[xe, ye], [xs, ys]], np.int32)

        cv.line(img, (DirectionVec[0][0], DirectionVec[0][1]), (DirectionVec[1][0], DirectionVec[1][1]), (255, 0, 0),
                2)  # 绘制矩形线框居中的线段
        # cv.line(img, (int(img.shape[1]/2),int(img.shape[0])),(DirectionVec[1][0],DirectionVec[1][1]),(0,255,0),5)

        # if (DirectionVec[1][0] != DirectionVec[0,0])and(DirectionVec[1][1] != DirectionVec[0][1]):
        angle = np.arccos((DirectionVec[0][1] - DirectionVec[1][1]) / np.sqrt(
            np.square(DirectionVec[1][0] - DirectionVec[0, 0]) + np.square(
                DirectionVec[1][1] - DirectionVec[0][1])))  # abs(ye-ys)/aprt(square(xe-xs)+square(ye-ys))
        angle = int(angle / np.pi * 180)  # 转成角度值
        # print("angle",angle)

        if True == robotStart:
            angleLast = angle
        #     robotStart = False
        # if abs(angle - angleLast) > 20:
        #     angle = angleLast
        # else:
        #     angle = angleLast
        # angleLast = angle

        # angleList.append(angle)
        # if len(angleList) > 15:
        #     maxIndex = angleList.index(max(angleList))
        #     minIndex = angleList.index(min(angleList))
        #     angleList[maxIndex] = 0
        #     angleList[minIndex] = 0
        #     angle = np.mean(angleList)
        #     angleList = []

        angle = '%d' % angle
        # serialSendData(data = angle.encode())

        if ys > ye:
            if xs < xe:
                angle = '-' + angle
                # print("angle",angle)
                # s = str('-')
                # serialSendData(data = s.encode()+angle.encode())
            # else:
            # print("angle",angle)
            # serialSendData(data = angle.encode())
        else:
            if xs > xe:
                angle = '-' + angle
                # print("angle",angle)
                # s = str('-')
                # serialSendData(data = s.encode()+angle.encode())
            # else:
            # print("angle",angle)
        return angle
    else:
        return None


def targetRecognition(imgBinary):
    global findedTarg

    contours, _ = cv.findContours(imgBinary, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE)
    area = []
    numLines = []
    if len(contours) > 0:
        for index in range(len(contours)):
            area.append(cv.contourArea(contours[index]))
        # print(max(area))
        for index in range(len(contours)):
            if max(area) > cv.contourArea(contours[index]) and max(area) / 300 < cv.contourArea(
                    contours[index]):  # 舍弃最大的区域和较小的区域
                epsilon = 0.05 * cv.arcLength(contours[index], True)
                dpcnt = cv.approxPolyDP(contours[index], epsilon, True)
                if dpcnt is not None:
                    x = []
                    y = []
                    for point in dpcnt:
                        x.append(point[0][0])
                        y.append(point[0][1])
                    xMean = int(np.mean(x))
                    yMean = int(np.mean(y))
                    targPix = imgBinary[yMean - 3:yMean + 3, xMean - 3:xMean + 3]
                    meanPix = np.mean(targPix)
                    # print("meanPix",meanPix)
                    if meanPix < 50:
                        xMax = int(np.max(x))
                        yMax = int(np.max(y))
                        xMin = int(np.min(x))
                        yMin = int(np.min(y))
                        leftTop = (xMin, yMin)
                        leftBottom = (xMin, yMax)
                        rightTop = (xMax, yMin)
                        rightBottom = (xMax, yMax)
                        cv.rectangle(img, leftTop, rightBottom, (0, 0, 0), 2)
                        cv.circle(img, (xMean, yMean), 5, (255, 0, 255), 3)

                        padding = 10
                        # targRoi = np.zeros([yMax - yMin + 2*padding, xMax - xMin + 2*padding], np.uint8)
                        targRoi = imgBinary[yMin - padding:yMax + padding, xMin - padding:xMax + padding]

                        lines = HoughLines(targRoi)
                        if lines is not None:
                            for line in lines:
                                x1, y1, x2, y2 = line[0]
                                cv.line(targRoi, (x1, y1), (x2, y2), 150, 2)
                            # numLines.append(len(lines)*yMean/(0.8*imHeight))
                            numLines.append(len(lines)*yMean/(0.8*imHeight))
                            cv.imshow("targ", targRoi)

                        if yMean < 0.8 * imHeight:
                            findedTarg = False
                        if yMean > 0.8 * imHeight and not findedTarg:
                            numL = np.mean(numLines)
                            print("NumL: ", numL)
                            if numL > 3.0:
                                cv.putText(img, "square", leftTop, cv.FONT_HERSHEY_COMPLEX, 2.0, (100, 200, 200), 3)
                                print("square")
                                # serialSendData(data=b"red")

                            else:
                                cv.putText(img, "circle", leftTop, cv.FONT_HERSHEY_COMPLEX, 2.0, (100, 0, 200), 3)
                                print("circle")
                                # serialSendData(data=b"green")

                            findedTarg = True

                    cv.drawContours(img, dpcnt, -1, (0, 255, 255), 8)
        # cv.drawContours(img,[cnt],0,(0,255,255),2)


def roiMaskGenerate(imH, imW):
    roi_mask = np.zeros((imH, imW), dtype=np.uint8)
    ratioL = 0.1
    ratioR = 0.9
    ratioM = 0.4
    bottom_left = [ratioL * imW, imH]
    bottom_right = [ratioR * imW, imH]
    top_left = [ratioL * imW, ratioM * imH]
    top_right = [ratioR * imW, ratioM * imH]

    vertices = np.array([bottom_left, top_left, top_right, bottom_right], dtype=np.int32)
    rectMask = np.zeros((imH, imW), dtype=np.uint8)
    cv.fillPoly(rectMask, [vertices], 255)

    return rectMask


# ser = serial.Serial('COM3', 19200)  # ser = serial.Serial('/dev/ttyUSB0', 9600)//使用树莓派GPIO8端口作为串口时用AMA0,控制主控仓时，用USB0
# serialSendData(data=b"blue")

# cap = cv.VideoCapture(0)
cap = cv.VideoCapture("20210408205220.avi")
img = imRead(cap)

if img is None:
    print("imRead failed!")

imHeight, imWidth = img.shape[0], img.shape[1]

while True:
    img = imRead(cap)
    # t1 = time.time()img = imRead(cap)
    if img is None:
        break
    # cv.imshow("img", img)
    (cb, cg, cr) = cv.split(img)  # Red通道巡管,Blue通道识别形状
    # imgGrey = cv.cvtColor(img, cv.COLOR_BGR2GRAY)#将img图像进行灰度处理

    crFilted = imFiltering(cr)
    cbFilted = imFiltering(cb)
    # cv.imshow("cbFilted",cbFilted )#显示滤波后图像

    # clahe = cv.createCLAHE(clipLimit=2.0, tileGridSize=(8,8))
    # cbHistEq = clahe.apply(cbFilted)
    # cv.imshow("imgEqu",cbFilted )

    # 二值化图像01
    _, crThreshed = cv.threshold(crFilted, 200, 255, cv.THRESH_BINARY)  # mp4视频阈值150
    _, cbThreshed = cv.threshold(cbFilted, 140, 255, cv.THRESH_BINARY)  # mp4视频阈值100
    cv.imshow("cbThreshed", cbThreshed)

    robotAngle = robotDirect(crThreshed, robotStart)
    # response = ser.readline().strip()
    # if response == b"a":
    #     try:
    #         serialSendData(data=robotAngle.encode())
    #     except:
    #         pass

    kernel = np.ones((5, 5), np.uint8)
    # erosion = cv.erode(crThreshed,kernel1,iterations = 3)
    # dilation = cv.dilate(erosion,kernel2,iterations = 3)
    imMorphy = cv.morphologyEx(cbThreshed, cv.MORPH_OPEN, kernel)
    # imMorphy = cv.morphologyEx(cbThreshed, cv.MORPH_CLOSE, kernel)

    mask = roiMaskGenerate(imHeight, imWidth)
    # cv.imshow('mask',mask)
    imgRoi = cv.bitwise_and(imMorphy, imMorphy, mask=mask)
    # cv.imshow("ROI",imgRoi)

    targetRecognition(imgRoi)

    cv.imshow("img", img)
    # t2 = time.time()
    # print('程序运行时间:%s毫秒' % ((t2 - t1) * 1000))
    sleepTime = 0

    if cv.waitKey(50) == ord('q'):
        break
    time.sleep(sleepTime)

cap.release()
cv.destroyAllWindows()
